#include "problem.h"
#include<iostream>

#include <Eigen/Core>
#include <ceres/ceres.h>
#include <ceres/rotation.h>

#include <vector>
#include <cmath>
using namespace Eigen;
using namespace ceres;

double f = 115.0;
double x_0 = 0;
double y_0 = 0;

Vector3d intersection_similarity_transform(const DataIntersection &data) { 
	//1.计算基线初值
	Eigen::Vector3d B;
	B = data.X_eo[1] - data.X_eo[0];
	
	//2.计算像点坐标的像空间辅助坐标系坐标

	//3.计算点投影系数
	double N1,N2;

	//4.计算地面坐标
	Eigen::Vector3d XA;

	return XA; 
}

Vector3d intersection_least_squares(const DataIntersection &data, const Vector3d &X_initial) { 
	// 已知值x0,y0,f,xs,ys,zs,phi,omga,kappa
	// 观测值x1,y1,x2,y2
	// 未知数xA,yA,zA
	Vector3d Xi = X_initial;
		
	return Xi;
}